#include <AT91SAM7S256.h>

#define AT91C_TWI_CLDIV_SHIFT 0

#define AT91C_TWI_CHDIV_SHIFT 8

#define AT91C_TWI_CKDIV_SHIFT 16


void TWI_Setup(void){
	
	AT91F_TWI_CfgPMC();
	AT91F_TWI_CfgPIO();
	
	AT91F_TWI_Configure(AT91C_BASE_TWI);
	
	// TWI Clock MCK = 30 MHz, TWI = 8kHz
	*AT91C_TWI_CWGR = 0x047575;

	
	
	/*AT91C_BASE_TWI->TWI_CWGR = AT91C_TWI_CLDIV & (8 << AT91C_TWI_CLDIV_SHIFT)		// Clock low divider
								| AT91C_TWI_CHDIV & (8 << AT91C_TWI_CHDIV_SHIFT)	// Clock High divider
								| AT91C_TWI_CKDIV & (8 << AT91C_TWI_CKDIV_SHIFT);		// Clock divider
	
	*/
	// Device address 0x50, addressable space 16 bits
	//*AT91C_TWI_MMR = 0x500200;
	
	AT91C_BASE_TWI->TWI_MMR = (AT91C_TWI_DADR & (0x50 << 16)) 		// Set device address
								| AT91C_TWI_IADRSZ_2_BYTE;			// Set 2 bytes for internal device address

}

void TWI_WriteByte(int address, char byte){
	int status;
	/*
	// Set internal device address
	AT91C_BASE_TWI->TWI_IADR = address;

	// Set write in mode register
	AT91C_BASE_TWI->TWI_MMR &= ~AT91C_TWI_MREAD;

	// Set control register
	AT91C_BASE_TWI->TWI_CR = AT91C_TWI_START
							| AT91C_TWI_MSEN
							| AT91C_TWI_STOP;

	// Set data register to start transmission
	AT91C_BASE_TWI->TWI_THR = byte;

	// Wait until the end of transmission
	while(!(AT91C_BASE_TWI->TWI_SR & AT91C_TWI_TXCOMP));
*/
	
	//* Set the Internal device address
	*AT91C_TWI_IADR = address;

	//* Set Write in mode register
	*AT91C_TWI_MMR &= 0xFFFFEFFF;

	//* Set control register
	*AT91C_TWI_CR = AT91C_TWI_START | AT91C_TWI_MSEN | AT91C_TWI_STOP;

	//* Set Data register for start transmission
	*AT91C_TWI_THR = byte;

	//* Wait end transmission
	status = *AT91C_TWI_SR ;
	while (!(status & AT91C_TWI_TXCOMP)){
	status = *AT91C_TWI_SR ; }
	


}

void TWI_WritePage(int address, char* page, char pageLen){
	char internalCount = 0;

	
	// Set internal device address
	AT91C_BASE_TWI->TWI_IADR = address;

	// Set write in mode register
	AT91C_BASE_TWI->TWI_MMR &= ~AT91C_TWI_MREAD;

	// Set control register
	AT91C_BASE_TWI->TWI_CR = AT91C_TWI_START
							| AT91C_TWI_MSEN;

	while(internalCount < pageLen){
		// Set data register to start transmission
		AT91C_BASE_TWI->TWI_THR = *(page+internalCount);

		// Wait until the transmit hold register is empty
		while(!(AT91C_BASE_TWI->TWI_SR & AT91C_TWI_TXRDY));

		//Increment internal count
		internalCount++;
	}

	// Send stop bit
	AT91C_BASE_TWI->TWI_CR |= AT91C_TWI_STOP;

	// Wait until the end of transmission
	while(!(AT91C_BASE_TWI->TWI_SR & AT91C_TWI_TXCOMP));
	




}

void TWI_ReadBytes(int address, char* data, char numBytes){
	char internalCount = 0;
	int status;
	
	// Set internal device address
	AT91C_BASE_TWI->TWI_IADR = address;

	// Set read in mode register
	AT91C_BASE_TWI->TWI_MMR |= AT91C_TWI_MREAD;

	// Set control register
	AT91C_BASE_TWI->TWI_CR = AT91C_TWI_START
								| AT91C_TWI_MSEN;

	while(internalCount < numBytes){

		// Wait until the receive holding register is full
		while(!(AT91C_BASE_TWI->TWI_SR & AT91C_TWI_RXRDY));

			// Store data in pointer
			*(data+internalCount) = AT91C_BASE_TWI->TWI_RHR;

			//Increment internal count
			internalCount++;
		}

	// Send stop bit
	AT91C_BASE_TWI->TWI_CR |= AT91C_TWI_STOP;

	// Wait until the end of transmission
	while(!(AT91C_BASE_TWI->TWI_SR & AT91C_TWI_TXCOMP));
	
	//dummy read on register
	status = AT91C_BASE_TWI->TWI_RHR;
	
}

